/*
 * Copyright (c) 2006-2022, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021-08-20     BruceOu      first implementation
 */

#include <stdio.h>
#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
#include "drivers/PWM_input_capture.h"
#include "BLDCMotor.h"
#include "BLDCDriver6PWM.h"
#include "MagneticSensorPWM.h"
#include "InlineCurrentSense.h"

//void MotorLEncoder_thread_entry(void *parameter)
//{
//    rt_device_t Lpwm_input_dev;
//    rt_uint16_t wait_i;
//    struct pwm_inputcapture_device *inputcap_dev;
//    Lpwm_input_dev = rt_device_find("pwm_inputcap1");
//    if(Lpwm_input_dev == RT_NULL)
//        return;
//    inputcap_dev = (struct pwm_inputcapture_device *)Lpwm_input_dev;
//    rt_device_open(Lpwm_input_dev, RT_DEVICE_OFLAG_RDONLY);
//    while(1)
//    {
//        if(inputcap_dev->sem != RT_NULL)
//        {
//            rt_sem_take(inputcap_dev->sem, RT_WAITING_FOREVER);
//            if(wait_i++ >= 1000)
//            {
//                rt_kprintf("MotorL encoder:\t\t%d\n",inputcap_dev->pulse_param.pulsewidth_us * 10000 / inputcap_dev->pulse_param.pulsecycle_us);
//                wait_i = 0;
//            }
//        }
//    }
//}
//
//void MotorREncoder_thread_entry(void *parameter)
//{
//    rt_device_t Rpwm_input_dev;
//    rt_uint16_t wait_i;
//    struct pwm_inputcapture_device *inputcap_dev;
//    Rpwm_input_dev = rt_device_find("pwm_inputcap3");
//    if(Rpwm_input_dev == RT_NULL)
//        return;
//    inputcap_dev = (struct pwm_inputcapture_device *)Rpwm_input_dev;
//    rt_device_open(Rpwm_input_dev, RT_DEVICE_OFLAG_RDONLY);
//    while(1)
//    {
//        if(inputcap_dev->sem != RT_NULL)
//        {
//            rt_sem_take(inputcap_dev->sem, RT_WAITING_FOREVER);
//            if(wait_i++ >= 1000)
//            {
//                rt_kprintf("MotorR encoder:%d\n",inputcap_dev->pulse_param.pulsewidth_us * 10000 / inputcap_dev->pulse_param.pulsecycle_us);
//                wait_i = 0;
//            }
//        }
//    }
//}

/* defined the LED2 pin: PC6 */
#define LED_PIN GET_PIN(B, 11)
#define BUZZER_PIN GET_PIN(B, 12)

BLDCMotor Motor_left;
BLDCDriver6PWM Motor_left_driver;
MagneticSensorPWM sensor_pwm;
InlineCurrentSense current_sense;
float target_position = 0;
float target_velocity = 6.28;

void motor_test_timeout(void *parameter)
{
    Motor_left.foc_motor.ops.move(&(Motor_left.foc_motor), *(float*)(parameter));
}

void MagneticSensorPWM_callback()
{
    Motor_left.foc_motor.ops.loopFOC(&(Motor_left.foc_motor));
    Motor_left.foc_motor.ops.move(&(Motor_left.foc_motor), target_velocity);
}

int main(void)
{
    int count = 1;
    rt_timer_t motor_tm;
    uint8_t send_buf[64], led_sta=0;
    rt_tick_t old_tick=0, now_tick=0;
    float motor_velocity = 0;

    /* set LED2 pin mode to output */
    rt_pin_mode(LED_PIN, PIN_MODE_OUTPUT);
	
    rt_pin_write(BUZZER_PIN, PIN_HIGH);
    rt_pin_mode(BUZZER_PIN, PIN_MODE_OUTPUT);
	rt_thread_delay(100);
	rt_pin_write(BUZZER_PIN, PIN_LOW);
    rt_thread_delay(100);
    rt_pin_write(BUZZER_PIN, PIN_HIGH);
    rt_thread_delay(100);
    rt_pin_write(BUZZER_PIN, PIN_LOW);
	
	rt_kprintf("FOC MiniCar App running!\n");
	rt_thread_delay(1000);

	BLDCMotor_set_default(&Motor_left, 7,  8, 100, NOT_SET);
	BLDCDriver6PWM_set_default(&Motor_left_driver, RT_USING_PWM1_NAME, RT_USING_PWM2_NAME, RT_USING_PWM3_NAME, RT_USING_PWM1_CH, RT_USING_PWM2_CH, RT_USING_PWM3_CH, 10000);
	MagneticSensorPWM_set_default(&sensor_pwm, "MotorL_sensor","pwm_inputcap1");
	InlineCurrentSense_set_default_by_gain(&current_sense, 0.01f, 50, "adc0", 10, 11, NOT_SET);

	current_sense.current_sense.ops.init(&(current_sense.current_sense));

    sensor_pwm.sensor.ops.init(&(sensor_pwm.sensor));
    FOCMotor_linkSensor(&(Motor_left.foc_motor), &(sensor_pwm.sensor));
    FOCMotor_linkCurrentSense(&(Motor_left.foc_motor), &(current_sense.current_sense));

	Motor_left_driver.bldc_driver.voltage_power_supply = 8;
	Motor_left_driver.bldc_driver.voltage_limit = 8;
	Motor_left_driver.bldc_driver.ops.init(&(Motor_left_driver.bldc_driver));

	Motor_left.linkDriver(&Motor_left, &(Motor_left_driver.bldc_driver));
    CurrentSense_linkDriver(&(current_sense.current_sense), &(Motor_left_driver.bldc_driver));

	Motor_left.foc_motor.voltage_limit = 4;
//	Motor_left.foc_motor.velocity_limit = 50;
	Motor_left.foc_motor.controller = velocity;
    Motor_left.foc_motor.PID_velocity.output_ramp = 1000;
    Motor_left.foc_motor.PID_velocity.P = 0.04;
    Motor_left.foc_motor.PID_velocity.I = 0.4;//20;
    Motor_left.foc_motor.PID_velocity.D = 0;
    Motor_left.foc_motor.LPF_velocity.Tf = 0.01f;
	Motor_left.foc_motor.ops.init(&(Motor_left.foc_motor));
	Motor_left.foc_motor.ops.initFOC(&(Motor_left.foc_motor), NOT_SET, CW);

    MagneticSensorPWM_enableInterrupt(&sensor_pwm, &MagneticSensorPWM_callback);

//	motor_tm = rt_timer_create("motor_test_tm", motor_test_timeout, &target_velocity, 10, RT_TIMER_FLAG_SOFT_TIMER | RT_TIMER_FLAG_PERIODIC);
//	if(motor_tm != RT_NULL)
//	    rt_timer_start(motor_tm);

//	pwm1_LA = rt_device_find(RT_USING_PWM1_NAME);
//	if(pwm1_LA != RT_NULL)
//	{
//	    struct rt_device_pwm *pwm_dev = (struct rt_device_pwm *)pwm1_LA;
//	    rt_pwm_set(pwm_dev,RT_USING_PWM1_CH+1,10000,5000);
//	    rt_pwm_enable(pwm_dev,-(RT_USING_PWM1_CH+1));
//	    rt_kprintf("%s init OK!\n", pwm_dev->parent.parent.name);
//	}
//
//    pwm2_LB = rt_device_find(RT_USING_PWM2_NAME);
//    if(pwm2_LB != RT_NULL)
//    {
//        struct rt_device_pwm *pwm_dev = (struct rt_device_pwm *)pwm2_LB;
//        rt_pwm_set(pwm_dev,RT_USING_PWM2_CH+1,10000,1000);
//        rt_pwm_enable(pwm_dev,-(RT_USING_PWM2_CH+1));
//        rt_kprintf("%s init OK!\n", pwm_dev->parent.parent.name);
//    }
//
//    pwm3_LC = rt_device_find(RT_USING_PWM3_NAME);
//    if(pwm3_LC != RT_NULL)
//    {
//        struct rt_device_pwm *pwm_dev = (struct rt_device_pwm *)pwm3_LC;
//        rt_pwm_set(pwm_dev,RT_USING_PWM3_CH+1,10000,8000);
//        rt_pwm_enable(pwm_dev,-(RT_USING_PWM3_CH+1));
//        rt_kprintf("%s init OK!\n", pwm_dev->parent.parent.name);
//    }
//
//    pwm4_RA = rt_device_find(RT_USING_PWM4_NAME);
//    if(pwm4_RA != RT_NULL)
//    {
//        struct rt_device_pwm *pwm_dev = (struct rt_device_pwm *)pwm4_RA;
//        rt_pwm_set(pwm_dev,RT_USING_PWM4_CH+1,10000,5000);
//        rt_pwm_enable(pwm_dev,-(RT_USING_PWM4_CH+1));
//        rt_kprintf("%s init OK!\n", pwm_dev->parent.parent.name);
//    }
//
//    pwm5_RB = rt_device_find(RT_USING_PWM5_NAME);
//    if(pwm5_RB != RT_NULL)
//    {
//        struct rt_device_pwm *pwm_dev = (struct rt_device_pwm *)pwm5_RB;
//        rt_pwm_set(pwm_dev,RT_USING_PWM5_CH+1,10000,1000);
//        rt_pwm_enable(pwm_dev,-(RT_USING_PWM5_CH+1));
//        rt_kprintf("%s init OK!\n", pwm_dev->parent.parent.name);
//    }
//
//    pwm6_RC = rt_device_find(RT_USING_PWM6_NAME);
//    if(pwm6_RC != RT_NULL)
//    {
//        struct rt_device_pwm *pwm_dev = (struct rt_device_pwm *)pwm6_RC;
//        rt_pwm_set(pwm_dev,RT_USING_PWM6_CH+1,10000,8000);
//        rt_pwm_enable(pwm_dev,-(RT_USING_PWM6_CH+1));
//        rt_kprintf("%s init OK!\n", pwm_dev->parent.parent.name);
//    }

//	rt_thread_t MotorL_encoder_thread;
//	MotorL_encoder_thread = rt_thread_create("MotorLEncoder", MotorLEncoder_thread_entry, RT_NULL, 1024, 4, 20);
//	rt_thread_startup(MotorL_encoder_thread);
//
//	rt_thread_t MotorR_encoder_thread;
//	MotorR_encoder_thread = rt_thread_create("MotorREncoder", MotorREncoder_thread_entry, RT_NULL, 1024, 4, 20);
//    rt_thread_startup(MotorR_encoder_thread);

    while (1)
    {
        now_tick = rt_tick_get();
        if(now_tick - old_tick > 500)
        {
            rt_pin_write(LED_PIN, led_sta);
            old_tick = now_tick;
            led_sta = !led_sta;
        }

        motor_velocity = sensor_pwm.sensor.ops.getVelocity(&(sensor_pwm.sensor));

//        rt_pin_write(LED_PIN, PIN_HIGH);
//        rt_thread_mdelay(500);
//        rt_pin_write(LED_PIN, PIN_LOW);
//        rt_thread_mdelay(500);
//        target_position += 0.1;
//        sprintf(send_buf, "velocity:%.2f rad/s\n", motor_velocity);
//        rt_kprintf(send_buf);
//        Motor_left.foc_motor.ops.loopFOC(&(Motor_left.foc_motor));
//        Motor_left.foc_motor.ops.move(&(Motor_left.foc_motor), target_velocity);
        rt_thread_mdelay(500);
    }

    return RT_EOK;
}

//static int motorLA_pwm(int argc, char **argv)
//{
//    rt_err_t result = RT_EOK;
//    rt_uint32_t period, pulse;
//    struct rt_device_pwm *pwm_dev = (struct rt_device_pwm *)pwm1_LA;
//    if(argc >= 3)
//    {
//        period = atoi(argv[1]);
//        pulse = atoi(argv[2]);
//        rt_pwm_set(pwm_dev,RT_USING_PWM1_CH+1,period,pulse);
//        rt_kprintf("set %s:[period]%d-[pulse]%d!\n",pwm_dev->parent.parent.name, period, pulse);
//    }
//    else {
//        rt_kprintf("Usage: \n");
//        rt_kprintf("motorXN_pwm <period> <pulse> - set the pwm's period and pulse\n");
//        rt_kprintf("eg:motorLA_pwm 10000 5000 is set motorLA pwm's period to 10us, pulse to 5us\n");
//        result = -RT_ERROR;
//    }
//    return RT_EOK;
//}
//MSH_CMD_EXPORT(motorLA_pwm, motorLA_pwm <period> <pulse>);

static int motorL_V_PI(int argc, char **argv)
{
    rt_err_t result = RT_EOK;
    float p, i;
    char buf[64];
    if(argc >= 3)
    {
        p = atof(argv[1]);
        i = atof(argv[2]);
        Motor_left.foc_motor.PID_velocity.P = p;
        Motor_left.foc_motor.PID_velocity.I = i;
        sprintf(buf, "set P:%.3f,I%.3f\n", p, i);
        rt_kprintf(buf);
    }
    else {
        rt_kprintf("Usage: \n");
        rt_kprintf("motorL_V_PI <p_val> <i_val> - set the left motor velocity PI value\n");
        rt_kprintf("eg:motorL_V_PI 0.2 5 is set left motor velocity P to 0.2, I to 5\n");
        result = -RT_ERROR;
    }
    return RT_EOK;
}
MSH_CMD_EXPORT(motorL_V_PI, motorL_V_PI <p_val> <i_val>);

static int motorL_V(int argc, char **argv)
{
    rt_err_t result = RT_EOK;
    float velocity;
    char buf[64];
    if(argc >= 2)
    {
        velocity = atof(argv[1]);
        target_velocity = velocity;
        sprintf(buf, "set velocity:%.3f\n", velocity);
        rt_kprintf(buf);
    }
    else {
        rt_kprintf("Usage: \n");
        rt_kprintf("motorL_V <velocity>  - set the left motor velocity\n");
        rt_kprintf("eg:motorL_V 6 is set left motor velocity to 6 rad/s\n");
        result = -RT_ERROR;
    }
    return RT_EOK;
}
MSH_CMD_EXPORT(motorL_V, motorL_V <velocity>);

